#include "robot_manager.h"
#include <sstream>
#include <vector>
#include "ros/ros.h"
#include "utilities/file.hpp"

void RobotManager::commandTeachMoveCallback(const std_msgs::Int32::ConstPtr& msg)
{
    //! 不支持仿真
    if(is_simulation_)
    {
        ROS_ERROR("simulation not support in this mode!");
        return;
    }
    if(msg->data == 1)
    {

        ROS_INFO("enable zero-force drag mode...");
        //! 切换到位置速度模式并使能电机
        // for (auto &motor: motors_)
        // {
        //     if (dm_->switchControlMode(motor, damiao::MIT_MODE))
        //         ROS_INFO("Switch motor %d to MIT_MODE Success", motor.GetSlaveId());
        //     dm_->enable(motor);
        // }
        ros::Rate rate(CONTROL_FREQ);
        std::vector<double> q, qd(7, 0.0), qdd(7, 0.0);
        std::vector<double> torque;
        std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};
        int k = 0;
        std::vector<std::vector<double>> q_sets;
        double time = 0.0;
        while (1)
        {
            pthread_mutex_lock(&zoros_mutex_);
            q = joint_state_msg_.position;
            robot_model_->inverseDynamics(q, qd, qdd, torque);
            // qd = joint_state_msg_.velocity;
            for(int i = 0; i < motors_.size(); i++)
            {
                dm_->control_mit(motors_[i], 0.0, 0.0, 0.0, 0.0, torque[i] * D[i]);
            }
            if(time < 10)
            {
                q_sets.push_back(q);
            }
            pthread_mutex_unlock(&zoros_mutex_);
            time += 0.004;
            rate.sleep();

            if(time > 10)
            {
                // WriteDataFile("./data_" + GetCurrentTimeString() + ".txt", q_sets, ',');
                ROS_INFO("record pass 10 seconds data!");
                q_sets.clear();
                time = 0;
            }
        }
    }
}
